

#pragma once

#include "rclcpp/rclcpp.hpp"
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include "rclcpp_action/rclcpp_action.hpp"
#include <Communication/SimpleActionClientD.h>
#include "tf2_ros/buffer.h"
#include <ipa_building_msgs/action/find_room_sequence_with_checkpoints.hpp>
#include <ipa_building_msgs/action/map_segmentation.hpp>
#include <ipa_building_msgs/action/room_exploration.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <nav_msgs/msg/path.hpp>
#include "nav2_msgs/srv/get_map_nav2.hpp"
#include <geometry_msgs/msg/pose.hpp>
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/utils.h"
#include "lib/pure_pursuit_controller.h"

using ClientSegmentation=ipa_building_msgs::action::MapSegmentation;
using ClientSequence=ipa_building_msgs::action::FindRoomSequenceWithCheckpoints;
using ClientExploration=ipa_building_msgs::action::RoomExploration;

class IpaRoomPlanning
{

public:
    struct IpaRoomPlanningParam
    {
        IpaRoomPlanningParam() {};
        std::string image_file = "";
        std::vector<geometry_msgs::msg::Point32> fov_points;
        std::vector<double> map_origin;
        std::vector<double> robot_pos;
        std::vector<double> start_pos;
        geometry_msgs::msg::Point32 fov_origin;
        double robot_radius = 0.15;
        double coverage_radius = 0.15;
        double map_resolution = 0.05;
        int planning_mode = 1;
        bool return_format_in_meter = false;
        bool return_format_in_pixel = true;
        bool segmentation_display = false;
        bool sequence_display = false;
        bool exploration_display = false;
        bool exploration_do_flip = true;
    };

    IpaRoomPlanning(rclcpp::Node::SharedPtr &nh, std::shared_ptr<tf2_ros::Buffer> &tf_);

    bool prepareData(std::vector<double> &robot_pos, std::string image_file = std::string(""));

    void fromPathGetMat(std::string image_path);

    void matToMap(nav_msgs::msg::OccupancyGrid &map, const cv::Mat &mat);

    void mapToMat(const nav_msgs::msg::OccupancyGrid &map, cv::Mat &mat);

    bool calRoomSegmentation(IpaRoomPlanningParam &segmentation_param);

    bool calRoomSequence(IpaRoomPlanningParam &sequence_param);

    bool calRoomExploration(IpaRoomPlanningParam &exploration_param, cv::Mat &exploration, cv::Mat &display_mat);

    bool calRoomSegmentation() { return calRoomSegmentation(ipa_planning_param_); };

    bool calRoomSequence() { return calRoomSequence(ipa_planning_param_); };

    bool calRoomExploration(cv::Mat &exploration, cv::Mat &display_mat)
    { return calRoomExploration(ipa_planning_param_, exploration, display_mat); };

    bool calVirtualWall(std::vector<std::vector<geometry_msgs::msg::PoseStamped>> *virtual_path = nullptr);

    void Binarization(cv::Mat &map_copy, int threshold, int min_value = 0, int max_value = 255);

    void Publish();

    void sectionPublish(std::vector<geometry_msgs::msg::PoseStamped> &poses_sub);

    bool Moving(double &linear_vel, double &angular_vel);

    void setPlan(std::vector<geometry_msgs::msg::PoseStamped> &path);

    double pathEpsSqr(double dis_ = 0.34)
    { return pow(path_eps + dis_, 2); };

    void cutOff(std::vector<geometry_msgs::msg::PoseStamped> &poses, std::vector<int> &ids);

    void downSample(std::vector<cv::Point2f> &path, double dis_between_point = 0.3, double dis_with_wall = 0);

    rclcpp::Client<nav2_msgs::srv::GetMapNav2>::SharedPtr getMapClient()
    { return grid_client; };

    bool getMap(std::shared_ptr<nav2_msgs::srv::GetMapNav2_Response> &srv_goal);

    bool dataDone() { return done_data; };

    bool roomSegmentationDone() { return done_room_segmentation; };

    bool roomSequenceDone() { return done_room_sequence; };

    std::vector<std::vector<geometry_msgs::msg::PoseStamped>> &getVirtualWall()
    { return all_virtual_path; };

    std::vector<int> &Sequence() { return optimal_room_sequence; };

    int &roomID() { return room_id; };

    int &maxRoom() { return max_room; };

    int roomExploration(int room_id_);

    bool LineFit(std::vector<cv::Point2f> &points, double &line_k, double &line_b);

    void GetInlier(const double &line_a, const double &line_b, const double &line_c,
                   const cv::Point2f &fit_point, cv::Point2f &result_point);

    double NormalizeAngle(double raw_angle)
    {
        if (-M_PI <= raw_angle && raw_angle <= M_PI)
        {
            return raw_angle;
        }
        while (raw_angle > M_PI)
        { raw_angle -= M_PI * 2; }
        while (raw_angle < -M_PI)
        { raw_angle += M_PI * 2; }

        return raw_angle;
    }

    IpaRoomPlanningParam &getParem()
    { return ipa_planning_param_; };

    geometry_msgs::msg::PoseStamped goalPose(bool global_frame = true, bool earse_this = false)
    { return controllerD->goalPose(global_frame, earse_this); };

    bool setNextGoal(geometry_msgs::msg::PoseStamped &pose_, double distance_sqr)
    { return controllerD->setNextGoal(pose_, distance_sqr); };


private:
    rclcpp::Node::SharedPtr node_handle_;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr all_path_pub, section_path_pub;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr test_path_pub;
    rclcpp::Client<nav2_msgs::srv::GetMapNav2>::SharedPtr grid_client;
    std::shared_ptr<SimpleActionClientD<ClientSegmentation>> room_segmentation_client;
    std::shared_ptr<SimpleActionClientD<ClientSequence>> room_sequence_client;
    std::shared_ptr<SimpleActionClientD<ClientExploration>> room_exploration_client;
    std::vector<std::vector<geometry_msgs::msg::PoseStamped>> all_virtual_path;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer = nullptr;
    std::shared_ptr<PurePursuitControllerD> controllerD;
    IpaRoomPlanningParam ipa_planning_param_;
    sensor_msgs::msg::Image input_image_msg;
    cv::Mat origin_map_mat;
    std::vector<int> optimal_room_sequence;
    double free_thresh;
    double occupied_thresh;

    int room_id = -1;
    int max_room = -1;
    bool use_test_image = false;
    bool done_data = false;
    bool online_map = false;
    bool done_room_segmentation = false;
    bool done_room_sequence = false;
public:
    std::shared_ptr<ClientSegmentation::Result> result_seg = nullptr;
    std::shared_ptr<ClientSequence::Result> result_seq = nullptr;
    std::shared_ptr<ClientExploration::Result> result_exp = nullptr;
    std::vector<std::vector<geometry_msgs::msg::PoseStamped>> all_room_paths;
    cv::Mat colour_segmented_map;
    cv::Mat exploration_map;
    cv::Mat display;
    double path_eps = 0.34;
    double path_eps_sqr = path_eps * path_eps;
};


